// Include files.
#include "TankStrafeDriveFilter.hpp"

/**
 *  Apply the filter to an input.
 */
CartesianValue TankStrafeDriveFilter::Apply(const TwoJoystickValue &input) const
{
	double x = (input.GetLeftValue().GetX() + input.GetRightValue().GetX()) / 2;
	double y = -((input.GetLeftValue().GetY() + input.GetRightValue().GetY()) / 2);
	double rotation = (input.GetRightValue().GetY() - input.GetLeftValue().GetY()) / 2;
	
	//printf("x input:%f, y input:%f, rotation:%f \n", value.m_x, value.m_y, value.m_rotation);
	//printf("Drive Mode: Two Joysticks, Tank Passive\n");
	return CartesianValue(x, y, rotation);
}
